#!/bin/bash

# 无人机导航系统一键启动脚本
# 适用于Jetson Orin Nano + ROS2 Humble + Livox Mid360

set -e  # 遇到错误立即退出

# 颜色定义
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
PURPLE='\033[0;35m'
CYAN='\033[0;36m'
NC='\033[0m' # No Color

# 脚本配置
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
WS_DIR="$SCRIPT_DIR"
LOG_DIR="$HOME/drone_navigation_logs"

print_banner() {
    echo -e "${CYAN}"
    echo "=================================================================="
    echo "           无人机自主导航系统 - 一键启动脚本"
    echo "          Jetson Orin Nano + ROS2 Humble + Livox Mid360"
    echo "=================================================================="
    echo -e "${NC}"
}

print_info() {
    echo -e "${BLUE}[信息]${NC} $1"
}

print_success() {
    echo -e "${GREEN}[成功]${NC} $1"
}

print_warning() {
    echo -e "${YELLOW}[警告]${NC} $1"
}

print_error() {
    echo -e "${RED}[错误]${NC} $1"
}

print_step() {
    echo -e "${PURPLE}[步骤]${NC} $1"
}

# 显示使用帮助
show_help() {
    echo "无人机导航系统一键启动脚本"
    echo ""
    echo "用法: $0 [启动模式] [选项]"
    echo ""
    echo "启动模式:"
    echo "  simple          简化启动（仅核心节点）"
    echo "  debug           调试启动（包含性能监控和可视化）"
    echo "  full            完整启动（包含所有传感器和调试工具）"
    echo "  monitor         仅启动性能监控"
    echo "  diagnostics     运行系统诊断"
    echo ""
    echo "选项:"
    echo "  --no-deps-check     跳过依赖检查"
    echo "  --no-build          跳过编译步骤"
    echo "  --log-level LEVEL   设置日志级别 (debug, info, warn, error)"
    echo "  --config FILE       指定配置文件"
    echo "  --serial            启用串口通信"
    echo "  --help, -h          显示此帮助信息"
    echo ""
    echo "示例:"
    echo "  $0 debug                    # 调试模式启动"
    echo "  $0 simple --no-build        # 简化启动，跳过编译"
    echo "  $0 full --log-level debug   # 完整启动，调试日志"
    echo ""
}

# 检查ROS2环境
check_ros2_environment() {
    print_step "检查ROS2环境..."
    
    if [ -z "$ROS_DISTRO" ]; then
        print_warning "ROS2环境变量未设置，尝试自动加载..."
        if [ -f "/opt/ros/humble/setup.bash" ]; then
            source /opt/ros/humble/setup.bash
            print_success "已加载ROS2 Humble环境"
        else
            print_error "未找到ROS2 Humble，请先安装ROS2"
            return 1
        fi
    else
        print_success "ROS2环境: $ROS_DISTRO"
    fi
    
    # 检查关键命令
    if ! command -v ros2 &> /dev/null; then
        print_error "ros2命令不可用"
        return 1
    fi
    
    if ! command -v colcon &> /dev/null; then
        print_error "colcon命令不可用，请安装: sudo apt install python3-colcon-common-extensions"
        return 1
    fi
    
    return 0
}

# 检查依赖
check_dependencies() {
    if [ "$SKIP_DEPS_CHECK" = "true" ]; then
        print_info "跳过依赖检查"
        return 0
    fi
    
    print_step "检查系统依赖..."
    
    local missing_deps=()
    
    # 检查ROS2包
    if ! ros2 pkg list | grep -q "navigation2"; then
        missing_deps+=("ros-humble-navigation2")
    fi
    
    if ! ros2 pkg list | grep -q "pcl_conversions"; then
        missing_deps+=("ros-humble-pcl-conversions")
    fi
    
    # 检查Python包
    python3 -c "import numpy, scipy, cv2, psutil" 2>/dev/null || missing_deps+=("python-packages")
    
    if [ ${#missing_deps[@]} -eq 0 ]; then
        print_success "所有依赖已满足"
        return 0
    else
        print_error "缺失以下依赖:"
        for dep in "${missing_deps[@]}"; do
            echo "  - $dep"
        done
        
        echo ""
        print_info "是否运行依赖安装脚本？"
        read -p "按回车继续，或输入 'n' 跳过: " -r
        
        if [[ ! $REPLY =~ ^[Nn]$ ]]; then
            if [ -f "$WS_DIR/install_dependencies.sh" ]; then
                print_info "运行依赖安装脚本..."
                bash "$WS_DIR/install_dependencies.sh"
            else
                print_error "未找到依赖安装脚本"
                return 1
            fi
        fi
    fi
}

# 编译工作空间
build_workspace() {
    if [ "$SKIP_BUILD" = "true" ]; then
        print_info "跳过编译步骤"
        return 0
    fi
    
    print_step "编译工作空间..."
    
    cd "$WS_DIR"
    
    # 检查是否需要编译
    if [ ! -d "install" ] || [ "src" -nt "install" ]; then
        print_info "开始编译..."
        
        # 设置编译参数（针对Jetson优化）
        export MAKEFLAGS="-j$(nproc --ignore=1)"  # 使用n-1个核心
        
        if colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release; then
            print_success "编译完成"
        else
            print_error "编译失败"
            return 1
        fi
    else
        print_success "工作空间已是最新"
    fi
    
    # 加载环境
    if [ -f "install/setup.bash" ]; then
        source install/setup.bash
        print_success "已加载工作空间环境"
    else
        print_error "未找到安装环境文件"
        return 1
    fi
}

# 检查外部依赖运行状态
check_external_dependencies() {
    print_step "检查外部依赖运行状态..."
    
    # 检查point_lio
    if ros2 topic list | grep -q "/Odometry"; then
        print_success "point_lio正在运行"
    else
        print_warning "point_lio未检测到，请确保已启动"
        echo "  启动命令: ros2 launch point_lio mapping.launch.py"
    fi
    
    # 检查Livox驱动
    if ros2 topic list | grep -q "/livox"; then
        print_success "Livox驱动正在运行"
    else
        print_warning "Livox驱动未检测到"
        echo "  启动命令: ros2 launch livox_ros_driver2 msg_MID360_launch.py"
        
        if [ "$START_MODE" = "full" ]; then
            print_info "完整模式将自动启动Livox驱动"
        fi
    fi
}

# 创建日志目录
setup_logging() {
    mkdir -p "$LOG_DIR"
    
    # 创建本次启动的日志文件
    LAUNCH_LOG="$LOG_DIR/launch_$(date +%Y%m%d_%H%M%S).log"
    
    print_info "日志将保存到: $LAUNCH_LOG"
}

# 启动导航系统
start_navigation_system() {
    print_step "启动导航系统 - 模式: $START_MODE"
    
    local launch_file=""
    local launch_args=""
    
    case "$START_MODE" in
        "simple")
            launch_file="navigation_simple.launch.py"
            launch_args=""
            ;;
        "debug")
            launch_file="drone_navigation_debug.launch.py"
            launch_args="debug:=true performance_monitor:=true rviz:=true"
            ;;
        "full")
            launch_file="drone_navigation_debug.launch.py"
            launch_args="debug:=true performance_monitor:=true rviz:=true use_point_lio:=true"
            ;;
        "monitor")
            print_info "启动性能监控器..."
            ros2 run drone_navigation performance_monitor &
            MONITOR_PID=$!
            print_success "性能监控器已启动 (PID: $MONITOR_PID)"
            
            print_info "按Ctrl+C停止监控..."
            wait $MONITOR_PID
            return 0
            ;;
        "diagnostics")
            print_info "运行系统诊断..."
            python3 "$WS_DIR/src/drone_navigation/scripts/system_diagnostics.py"
            return 0
            ;;
        *)
            print_error "未知启动模式: $START_MODE"
            return 1
            ;;
    esac
    
    # 添加通用参数
    if [ -n "$LOG_LEVEL" ]; then
        launch_args="$launch_args log_level:=$LOG_LEVEL"
    fi
    
    if [ -n "$CONFIG_FILE" ]; then
        launch_args="$launch_args config_file:=$CONFIG_FILE"
    fi
    
    if [ "$ENABLE_SERIAL" = "true" ]; then
        launch_args="$launch_args enable_serial:=true"
    fi
    
    # 构建启动命令
    local launch_cmd="ros2 launch drone_navigation $launch_file $launch_args"
    
    print_info "启动命令: $launch_cmd"
    print_info "日志文件: $LAUNCH_LOG"
    echo ""
    
    # 显示快捷键提示
    print_info "快捷键:"
    echo "  Ctrl+C  - 停止系统"
    echo "  Ctrl+Z  - 暂停到后台"
    echo ""
    
    # 启动系统
    print_success "正在启动导航系统..."
    
    # 使用tee同时输出到终端和日志文件
    $launch_cmd 2>&1 | tee "$LAUNCH_LOG"
}

# 清理函数
cleanup() {
    print_info "正在清理..."
    
    if [ -n "$MONITOR_PID" ]; then
        kill $MONITOR_PID 2>/dev/null || true
    fi
    
    # 终止所有相关进程
    pkill -f "drone_navigation" 2>/dev/null || true
    
    print_success "清理完成"
}

# 主函数
main() {
    # 设置信号处理
    trap cleanup EXIT
    trap 'print_info "收到中断信号，正在停止..."; exit 0' INT TERM
    
    print_banner
    
    # 解析命令行参数
    START_MODE="debug"  # 默认模式
    SKIP_DEPS_CHECK="false"
    SKIP_BUILD="false"
    LOG_LEVEL=""
    CONFIG_FILE=""
    ENABLE_SERIAL="false"
    
    while [[ $# -gt 0 ]]; do
        case $1 in
            simple|debug|full|monitor|diagnostics)
                START_MODE="$1"
                shift
                ;;
            --no-deps-check)
                SKIP_DEPS_CHECK="true"
                shift
                ;;
            --no-build)
                SKIP_BUILD="true"
                shift
                ;;
            --log-level)
                LOG_LEVEL="$2"
                shift 2
                ;;
            --config)
                CONFIG_FILE="$2"
                shift 2
                ;;
            --serial)
                ENABLE_SERIAL="true"
                shift
                ;;
            --help|-h)
                show_help
                exit 0
                ;;
            *)
                print_error "未知参数: $1"
                show_help
                exit 1
                ;;
        esac
    done
    
    print_info "启动模式: $START_MODE"
    print_info "工作目录: $WS_DIR"
    echo ""
    
    # 执行启动流程
    if ! check_ros2_environment; then
        exit 1
    fi
    
    if ! check_dependencies; then
        exit 1
    fi
    
    if ! build_workspace; then
        exit 1
    fi
    
    setup_logging
    
    check_external_dependencies
    
    echo ""
    print_success "环境检查完成，准备启动系统..."
    echo ""
    
    # 启动系统
    start_navigation_system
}

# 检查是否直接执行脚本
if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then
    main "$@"
fi 